#include "tim.h"
#include "i2c.h"
#include "general_task_include.h"
#include "CAN_driver.h"
#include "dt7_remote_driver.h"
//#include "ekf_imu_solver.h"
//#include "judge_receive.h"

//串口接收数据缓冲区
uint8_t uart1RecBuffer[UART1_MAX_RECEIVE_BUFFER_LENGTH];
uint8_t uart3RecBuffer[UART3_MAX_RECEIVE_BUFFER_LENGTH];	
uint8_t uart6RecBuffer[UART6_MAX_RECEIVE_BUFFER_LENGTH];
/*--------------------------------------------------remote task region------------------------------------------------*/
static void NormRemoteCmdInit(NormRemoteCmd* norm_remote_cmd);
static void DT7ToNormCmd(NormRemoteCmd* norm_remote_cmd, const DT7CmdData* dt7_cmd_data);



/*遥操作接收相关变量*/
EventGroupHandle_t remoteRecEventGroup;		//remote_task数据来源相关事件组

DT7RecData dt7RecData;	//完成数据拼接及移位操作后的dt7指令

DT7CmdData dt7CmdData;	//完全解读后的dt7遥控器指令
const DT7CmdData* _dt7CmdData = &dt7CmdData;
uint8_t dt7RecBuffer[DT7_RC_FRAME_LEN];	//dt7原始数据接收区

//NormRemoteCmd normRemoteCmd;	//将不同控制信号来源的信息整理归化到统一结构体中
//const NormRemoteCmd* _normRemoteCmd = &normRemoteCmd;

 /**
 * @brief 遥操作指令接收任务，处理接收遥控器数据，自定义控制器等
 */
 	EventBits_t currentEventGroupBits;	
void RemoteRecTask(void* argument)
{
	/*事件组初始化*/
	remoteRecEventGroup = xEventGroupCreate();	
	
	/*任务周期相关计算，并将其绑定到TaskMonitor相关指针中*/
	static uint32_t last_tick_count, current_tick_count, this_tick_count = 0;
	static uint16_t task_counter = 0;
	
	
	current_tick_count = last_tick_count = xTaskGetTickCount();
	
//	NormRemoteCmdInit(&normRemoteCmd);
	
	while(1)
	{
		currentEventGroupBits = xEventGroupWaitBits(remoteRecEventGroup,		/* 事件组的句柄 */
												    EVENT_GROUP_BIT_ERROR|EVENT_GROUP_BIT_DT7,	/* 检测事件标志位 */
													pdTRUE,						/* 满足添加时清除上面的事件位 */
													pdFALSE, 					/* 任意事件位被设置就会退出阻塞态--实现逻辑或 */
													portMAX_DELAY);				/* 没有超时 */
		task_counter++;
		
//		/*根据当前接收到的信号数据来源作相应的处理：
//		1.根据各自协议解码；
//		2.将各自解码得到的控制指令抄到标准控制指令中*/
//		if(currentEventGroupBits >= EVENT_GROUP_BIT_DT7)
//		{
//			DT7RawDataUpdate(&dt7RecData, dt7RecBuffer);
//			DT7DataProcess(&dt7CmdData, &dt7RecData);
//			DT7ToNormCmd(&normRemoteCmd, &dt7CmdData);
//		}
//		else if(currentEventGroupBits >= EVENT_GROUP_BIT_ERROR)
//		{
//			NormRemoteCmdInit(&normRemoteCmd);			
//			RemoteRecRestart();
//		}

				
		/*计算任务实际运行周期*/
		current_tick_count = xTaskGetTickCount();
		this_tick_count = current_tick_count - last_tick_count;
		last_tick_count = current_tick_count;
		
	}
}

// 控制 LED 灯


/*---------------------------------------------------motor CAN receive region-------------------------------------------*/
/*使用CAN总线通信的的电机的接收结构体*/

LKMotorRec shoulderMotorRec[2];
const LKMotorRec* _shoulderMotorRec = shoulderMotorRec;
LKMotorRec elbowMotorRec[2];
const LKMotorRec* _elbowMotorRec = elbowMotorRec;
LKMotorRec hipMotorRec[2];
const LKMotorRec* _hipMotorRec = hipMotorRec;
LKMotorRec kneeMotorRec[2];
const LKMotorRec* _kneeMotorRec = kneeMotorRec;

/**
 * @brief CAN1总线接收
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
  CAN_RxHeaderTypeDef RxHeader;                                      
  uint8_t aData[8];                                                  
  HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &RxHeader, aData);   
	switch(RxHeader.StdId)
	{	
		case 0x141:
		case 0x142:
			shoulderMotorRec[RxHeader.StdId-0x141].frame_counter++;
			if(aData[0] == 0xA1)
			{
				shoulderMotorRec[RxHeader.StdId-0x141].temperature = (int8_t)aData[1];
				shoulderMotorRec[RxHeader.StdId-0x141].iq = (int16_t)(aData[2] | aData[3] << 8);
				shoulderMotorRec[RxHeader.StdId-0x141].speed_rps = ((int16_t)(aData[4] | aData[5] << 8)) / 360.0 / I36; //电机带1:36的减速比，原始数据为dps
				shoulderMotorRec[RxHeader.StdId-0x141].encoder = (uint16_t)(aData[6] | aData[7] << 8);
			}	
		break;
		case 0x143:
		case 0x144:
			elbowMotorRec[RxHeader.StdId-0x143].frame_counter++;
			if(aData[0] == 0xA1)
			{
				elbowMotorRec[RxHeader.StdId-0x143].temperature = (int8_t)aData[1];
				elbowMotorRec[RxHeader.StdId-0x143].iq = (int16_t)(aData[2] | aData[3] << 8);
				elbowMotorRec[RxHeader.StdId-0x143].speed_rps = ((int16_t)(aData[4] | aData[5] << 8)) / 360.0 / I10; //电机带1:10的减速比，原始数据为dps
				elbowMotorRec[RxHeader.StdId-0x143].encoder = (uint16_t)(aData[6] | aData[7] << 8);
			}	
		break;
		case 0x145:
		case 0x146:
			hipMotorRec[RxHeader.StdId-0x145].frame_counter++;
			if(aData[0] == 0xA1)
			{
				hipMotorRec[RxHeader.StdId-0x145].temperature = (int8_t)aData[1];
				hipMotorRec[RxHeader.StdId-0x145].iq = (int16_t)(aData[2] | aData[3] << 8);
				hipMotorRec[RxHeader.StdId-0x145].speed_rps = ((int16_t)(aData[4] | aData[5] << 8)) / 360.0 / I10; //电机带1:10的减速比，原始数据为dps
				hipMotorRec[RxHeader.StdId-0x145].encoder = (uint16_t)(aData[6] | aData[7] << 8);
			}	
		break;
		case 0x147:
		case 0x148:
			kneeMotorRec[RxHeader.StdId-0x147].frame_counter++;
			if(aData[0] == 0xA1)
			{
				kneeMotorRec[RxHeader.StdId-0x147].temperature = (int8_t)aData[1];
				kneeMotorRec[RxHeader.StdId-0x147].iq = (int16_t)(aData[2] | aData[3] << 8);
				kneeMotorRec[RxHeader.StdId-0x147].speed_rps = ((int16_t)(aData[4] | aData[5] << 8)) / 360.0 / I9; //电机带1:9的减速比，原始数据为dps
				kneeMotorRec[RxHeader.StdId-0x147].encoder = (uint16_t)(aData[6] | aData[7] << 8);
			}	
		break;		

	}		
	
}

/**
 * @brief CAN2总线接收
 */
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
  CAN_RxHeaderTypeDef RxHeader;                                      
  uint8_t aData[8];                                                  
  HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &RxHeader, aData);   
	switch(RxHeader.StdId)
	{
		
		
	}
}

/*---------------------------------------------------------------------------imu task(if using onboard imu)-----------------------------------------------------------------------------------*/ 
/*板载运算--采用ekf计算姿态角*/
//IMUUseEKFSolver imuUseEKFSolver;
//const IMUUseEKFSolver *_imuUseEKFSolver = &imuUseEKFSolver;
//IMURecData imuRecData;
//const IMURecData *_imuRecData = &imuRecData;

/*云台姿态记录*/
//Pose gimbalPose;
//const Pose* _gimbalPose = &gimbalPose;

///*外接imu数据*/

///**
//  * @brief  imu温控
//  * @param  none
//  * @retval none
//  * 
//  */
//static void OnboardIMUTemperatureControl(int16_t real_temperature)
//{

//	if (real_temperature >= IMU_TARGET_TEMPERATURE)
//	{
//		__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1,HEAT_MIN);
//	}
//	else
//	{
//		__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1,HEAT_MID);
//	}
//}

// 
///**
// * @brief 更新姿态角结构体
// * @param IMUfromEKF IMUfromMahony
// * @note  根据当前使用的宏确定姿态角来源
// */

//#define X 0
//#define Y 1
//#define Z 2

//static void PoseUpdateFromIMU(Pose* pose, const IMUUseEKFSolver* imu_use_ekf)
//{
//	/*注意根据实际安装方向和自己取的正方向调整*/
//	//规定pitch轴抬头为正，低头为负；yaw轴顺时针旋转为正，逆时针旋转为负
//	#if defined ONBOARD_EKF_SOLVE
//		
//		pose->pitch_radps  = imu_use_ekf->Gyro[X];
//		pose->roll_radps   = imu_use_ekf->Gyro[Y];
//		pose->yaw_radps    = -imu_use_ekf->Gyro[Z];	
//	
//		pose->pitch_d = imu_use_ekf->Pitch_d;
//		pose->roll_d  = imu_use_ekf->Roll_d;	
//		pose->yaw_d	  = -imu_use_ekf->Yaw_d;

//	
//	#else

//	#endif
//	GimbalPoseUpdate(pose->pitch_d, pose->pitch_radps, pose->yaw_d, pose->yaw_radps, pose->roll_d, pose->pitch_radps);
//}

/**
 * @brief 从imu获取姿态角任务
 */
//void IMUTask(void* argument)
//{
//	/*任务周期相关计算，并将其绑定到TaskMonitor相关指针中*/
//	static uint32_t last_tick_count, current_tick_count, this_tick_count = 0;
//	static uint16_t task_counter = 0;
//	_taskMonitor->TaskFrameCounterPtr._imu_task = &task_counter;
//	_taskMonitor->TaskRunPeriodPtr._imu_task = &this_tick_count;
//	
//	current_tick_count = last_tick_count = xTaskGetTickCount();	
//	while(1)
//	{
//		/*任务主进程*/
//		#if defined ONBOARD_EKF_SOLVE
//			IMUSolverUseEKFUserFunc(&imuUseEKFSolver, &imuRecData);
//		#endif
//		OnboardIMUTemperatureControl(imuRecData.temperature);		
//		PoseUpdateFromIMU(&gimbalPose, &imuUseEKFSolver);

//		/*任务状态更新*/
//		task_counter++;
//		current_tick_count = xTaskGetTickCount();
//		this_tick_count = current_tick_count - last_tick_count;
//		last_tick_count = current_tick_count;
//		
//		vTaskDelayUntil(&current_tick_count, IMU_TASK_PERIOD_SET);
//	}
//}


/*--------------------------------------------------------------------------utility------------------------------------------------------------------------------------*/
/**
 * @brief 外设接收初始化
 */
\
uint8_t test[16];
void PeripheralRecEnable(void)
{
	/*DT7 remote*/
	HAL_UARTEx_ReceiveToIdle_DMA(&huart3, uart3RecBuffer, UART3_MAX_RECEIVE_BUFFER_LENGTH);	
	
	/*CAN Filter Init*/
	CANInitialize(&hcan1);
	CANInitialize(&hcan2);
	/*Enable CAN-receiving interrupt*/
	CANReceiveEnable(&hcan1);
	CANReceiveEnable(&hcan2);	
	
	/*video link remote init*/
	HAL_UARTEx_ReceiveToIdle_DMA(&huart1, uart1RecBuffer, UART1_MAX_RECEIVE_BUFFER_LENGTH);
//	HAL_UART_Receive_DMA(&huart1, uart1RecBuffer, 200);
	/*onboard imu init*/
//	#if defined ONBOARD_EKF_SOLVE
//		IMUSolverUseEKFInitialize(&imuUseEKFSolver, &imuRecData, IMU_TASK_PERIOD_SET / 1000.0f);
//	#endif

//	/*judge receive init*/
//	HAL_UARTEx_ReceiveToIdle_DMA(&huart6, uart6RecBuffer, UART6_MAX_RECEIVE_BUFFER_LENGTH);
	
	//调试，尝试定长接收
	HAL_UART_Receive_IT(&huart1,test,200);
}   

/**
 * @brief 串口dma不定长接收
 */
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{	
	
//	if(huart == &huart1)
//	{
//		uint8_t temp_buffer[Size];
//		memcpy(temp_buffer, uart1RecBuffer, Size);
//		memset(uart1RecBuffer,0, UART1_MAX_RECEIVE_BUFFER_LENGTH);
//		UpperCommRecHandler(temp_buffer, Size);
//		
//	}	
	
	if(huart == &huart3)
	{		
		if(DT7_RC_FRAME_LEN == Size)
		{
			memcpy(dt7RecBuffer, uart3RecBuffer, Size);
			BaseType_t xHigherPriorityTaskWoken = pdFALSE;		
			xEventGroupSetBitsFromISR(remoteRecEventGroup, EVENT_GROUP_BIT_DT7, &xHigherPriorityTaskWoken);
		}
		memset(uart3RecBuffer,0, UART3_MAX_RECEIVE_BUFFER_LENGTH);
		HAL_UARTEx_ReceiveToIdle_DMA(&huart3, uart3RecBuffer, UART3_MAX_RECEIVE_BUFFER_LENGTH);		
	}

//	else if(huart == &huart6)
//	{
//		uint8_t temp_buffer[Size];
//		memcpy(temp_buffer, uart6RecBuffer, Size);
//		memset(uart6RecBuffer,0, UART6_MAX_RECEIVE_BUFFER_LENGTH);
//		RefereeReceive(Size, temp_buffer);
//		
//		HAL_UARTEx_ReceiveToIdle_DMA(&huart6, uart6RecBuffer, UART6_MAX_RECEIVE_BUFFER_LENGTH);			
//	}
}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
  if(huart == &huart1)
  {
    __HAL_UART_CLEAR_FLAG(huart, UART_FLAG_RXNE);
    __HAL_UART_CLEAR_OREFLAG(huart);
    __HAL_UART_ENABLE_IT(huart,UART_IT_RXNE);
    __HAL_UART_ENABLE_IT(huart,UART_IT_ERR);
    huart->ErrorCode = HAL_UART_ERROR_NONE;
    huart->gState = HAL_UART_STATE_READY;
    huart->RxState = HAL_UART_STATE_READY;
  }
}                         